/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "filteringalgorithm.h"
#include <math.h>
#include <QDebug>

FilteringAlgorithm::FilteringAlgorithm()
{
    //---------初始化MC线程锁--------
//    pthread_mutexattr_init(&pthreadAttr);
//    pthread_mutexattr_setprotocol(&pthreadAttr,PTHREAD_PRIO_INHERIT);
//    pthread_mutex_init(&pthreadMutex,&pthreadAttr);
}

int FilteringAlgorithm::resetAll()
{
//    pthread_mutex_lock( &pthreadMutex);//没有出现segmentfault,但是好像锁没有起作用。
    QMutexLocker locker(&filterMutex);//segment fault
//    filterMutex.lock();
    dataQueue.clear();
//    filterMutex.unlock();
//     pthread_mutex_unlock( &pthreadMutex);
    return 1;
}

double FilteringAlgorithm::filterByForwardVel(double dataIn,double interpolationRatio,double maxJerkBias)
{

    QMutexLocker locker(&filterMutex);

    if(3>dataQueue.size())
    {
        dataQueue.enqueue(dataIn);
        return dataIn;
    }
    double tmpValue;

    double v1=dataQueue[2]-dataQueue[1];
    double v2=dataQueue[1]-dataQueue[0];
    double jerk1=v1-v2;
    double v3=dataIn-dataQueue[2];
    double jerk2=v3-v1;
    if(fabs(jerk2-jerk1)<maxJerkBias)
    {
        dataQueue.enqueue(dataIn);
        dataQueue.dequeue();

        return dataIn;
    }
    else
    {
        double newVel=v1+interpolationRatio*(jerk2+jerk1)/2;
        tmpValue=dataQueue[2]+newVel;
//        qDebug()<<"dataIn>maxValue:dataIn"<<dataIn<<"v1"<<v1
//                  <<"newVel"<<newVel <<"jerk1"<<jerk1 <<"jerk2"<<jerk2<<"dataQueue"<<dataQueue;

        dataQueue.enqueue(tmpValue);
        dataQueue.dequeue();

        return tmpValue;
    }


    return tmpValue;

}

double FilteringAlgorithm::filterByVelocityLimit(double dataIn, double deltaTimeIn,
                             double velocityFeedbackIn,  int bufferSize)
{
    if(bufferSize<0 || velocityFeedbackIn<0)
    {
        return dataIn;
    }
    QMutexLocker locker(&filterMutex);



    if(0==dataQueue.size())
    {
        dataQueue.enqueue(dataIn);
        return dataIn;
    }

    //速度限制
    double tmpMoved=dataIn-dataQueue.back();
    double tmpVel=fabs(tmpMoved)/deltaTimeIn;
    if(tmpVel<fabs(velocityFeedbackIn))
    {
        dataQueue.enqueue(dataIn);
    }
    else
    {
//        qDebug()<<"line vel filter limit trig";
        if(tmpMoved>0)
        {
            dataQueue.enqueue(dataQueue.back()+fabs(velocityFeedbackIn)*deltaTimeIn);
        }
        else
        {
            dataQueue.enqueue(dataQueue.back()-fabs(velocityFeedbackIn)*deltaTimeIn);
        }
    }


    while(dataQueue.size()>bufferSize)
    {
        dataQueue.dequeue();
    }


    return dataQueue.back();
}

double FilteringAlgorithm::filterByVelocityLimit_angle(double dataIn, double deltaTimeIn,
                                                       double velocityFeedbackIn, int bufferSize)
{
    if(bufferSize<0 || velocityFeedbackIn<0)
    {
        return dataIn;
    }
    QMutexLocker locker(&filterMutex);

    if(0==dataQueue.size())
    {
        dataQueue.enqueue(dataIn);
        return dataIn;
    }

    //角速度限制
    double angleBias=angleRotationJudgement(dataIn,dataQueue.back());
    double tmpVel=fabs(angleBias)/deltaTimeIn;
    if(tmpVel<fabs(velocityFeedbackIn))
    {
        dataQueue.enqueue(dataIn);
    }
    else
    {
//        qDebug()<<"angle vel filter limit trig";
        if(angleBias>0)
        {
            dataQueue.enqueue(dataQueue.back()+fabs(velocityFeedbackIn)*deltaTimeIn);
        }
        else
        {
            dataQueue.enqueue(dataQueue.back()-fabs(velocityFeedbackIn)*deltaTimeIn);
        }
    }


    while(dataQueue.size()>bufferSize)
    {
        dataQueue.dequeue();
    }


    return dataQueue.back();
}

double FilteringAlgorithm::arithmeticMeanfilter(double dataIn,int bufferSize)
{
    QMutexLocker locker(&filterMutex);
//    filterMutex.lock();
//    pthread_mutex_lock( &pthreadMutex);
    dataQueue.enqueue(dataIn);
    while(dataQueue.size()>bufferSize)
    {
        dataQueue.dequeue();
    }
    double tmpSum=0;
    for(int i=0;i<dataQueue.size();i++)
    {
        tmpSum+=dataQueue[i];
    }
    double tmpReturn=tmpSum/dataQueue.size();
//    filterMutex.unlock();
//    pthread_mutex_unlock( &pthreadMutex);
    return tmpReturn;
}

double FilteringAlgorithm::arithmeticMeanfilter_angle(double dataIn, int bufferSize)
{
    QMutexLocker locker(&filterMutex);
//    filterMutex.lock();
//    pthread_mutex_lock( &pthreadMutex);
    dataQueue.enqueue(dataIn);
    if(2>=dataQueue.size())
    {
        return dataIn;
    }
    while(dataQueue.size()>bufferSize)
    {
        dataQueue.dequeue();
    }


    //转换为角度距离
    QQueue<double> tmpQueue;
    for(int i=1;i<dataQueue.size();i++)
    {
//        tmpQueue.enqueue(angleRotationJudgement(dataQueue.front(),dataQueue[i]));//方向不对
        tmpQueue.enqueue(angleRotationJudgement(dataQueue[i],dataQueue.front()));
    }
    //角度偏差求平均
    double tmpSum=0;
    for(int i=0;i<tmpQueue.size();i++)
    {
        tmpSum+=tmpQueue[i];
    }
    double tmpAverage=tmpSum/bufferSize;

    return transformToPi(dataQueue.front()+tmpAverage);


}

double FilteringAlgorithm::arithmeticMeanfilter_angle(QVector<double> dataIn)
{
    if(1==dataIn.size())
    {
        return dataIn[0];
    }

    //转换为角度距离
    QQueue<double> tmpQueue;
    for(int i=1;i<dataIn.size();i++)
    {
//        tmpQueue.enqueue(angleRotationJudgement(dataQueue.front(),dataQueue[i]));//方向不对
        tmpQueue.enqueue(angleRotationJudgement(dataIn[i],dataIn[0]));
    }
    //角度偏差求平均
    double tmpSum=0;
    for(int i=0;i<tmpQueue.size();i++)
    {
        tmpSum+=tmpQueue[i];
    }
    double tmpAverage=tmpSum/dataIn.size();

    return transformToPi(dataIn[0]+tmpAverage);

}

double FilteringAlgorithm::arithmeticMeanfilter_low(double dataIn, int bufferSize)
{
    QMutexLocker locker(&filterMutex);
//    filterMutex.lock();
//    pthread_mutex_lock( &pthreadMutex);
    dataQueue.enqueue(dataIn);
    while(dataQueue.size()>bufferSize)
    {
        dataQueue.dequeue();
    }
    double tmpSum=0;
    for(int i=0;i<dataQueue.size();i++)
    {
        tmpSum+=dataQueue[i];
    }
    double tmpReturn=tmpSum/bufferSize;
//    filterMutex.unlock();
//    pthread_mutex_unlock( &pthreadMutex);
    return tmpReturn;

}



double FilteringAlgorithm::amplitudeLimiterFilter(double dataIn,double maxAmpliude)
{

   if(dataIn>maxAmpliude)
   {
       return maxAmpliude;
   }
   else if(dataIn<-maxAmpliude)
   {
       return -maxAmpliude;
   }
   else
   {
       return dataIn;
   }
   return dataIn;
}


double FilteringAlgorithm::middlevalueFilter(double dataIn,int bufferSize)
{//队列长度设置为奇数

    QMutexLocker locker(&filterMutex);
    dataQueue.enqueue(dataIn);
    while(dataQueue.size()>bufferSize)
    {
        dataQueue.dequeue();
    }

    int queueSize=dataQueue.size();
    if(queueSize<bufferSize)
    {
        return dataIn;
    }
    QQueue<double> tmpQueue=dataQueue;
    int j,k;
    double temp;
    for (j=0;j<queueSize-1;j++)
    {
        for (k=0;k<queueSize-1-j;k++)
        {
            if(tmpQueue[k]>tmpQueue[k+1])
             {
               temp = tmpQueue[k];
               tmpQueue[k] = tmpQueue[k+1];
               tmpQueue[k+1] = temp;
             }
        }
    }
    return tmpQueue[(queueSize-1)/2];
}

double FilteringAlgorithm::middlevalueFilter_angle(double dataIn, int bufferSize)
{
    QMutexLocker locker(&filterMutex);
    dataQueue.enqueue(dataIn);
    while(dataQueue.size()>bufferSize)
    {
        dataQueue.dequeue();
    }

    int queueSize=dataQueue.size();
    if(queueSize<bufferSize)
    {
        return dataIn;
    }
    //转换为角度距离
    QQueue<double> tmpQueue;
    for(int i=1;i<queueSize;i++)
    {
//        tmpQueue.enqueue(angleRotationJudgement(dataQueue.front(),dataQueue[i]));//方向不对
        tmpQueue.enqueue(angleRotationJudgement(dataQueue[i],dataQueue.front()));
    }

    //滤波算法
    queueSize=tmpQueue.size();
    int j,k;
    double temp;
    for (j=0;j<queueSize-1;j++)
    {
        for (k=0;k<queueSize-1-j;k++)
        {
            if(tmpQueue[k]>tmpQueue[k+1])
             {
               temp = tmpQueue[k];
               tmpQueue[k] = tmpQueue[k+1];
               tmpQueue[k+1] = temp;
             }
        }
    }
    return transformToPi(dataQueue.front()+tmpQueue[(queueSize-1)/2]);



}

double FilteringAlgorithm::maxAbsValueFilter(double dataIn, int bufferSize)
{
    QMutexLocker locker(&filterMutex);
    dataQueue.enqueue(dataIn);
    while(dataQueue.size()>bufferSize)
    {
        dataQueue.dequeue();
    }

    int queueSize=dataQueue.size();
    if(queueSize<bufferSize)
    {
        return dataIn;
    }

    double tmpMax=dataQueue.front();
    for (int j=1;j<queueSize;j++)
    {
        if(fabs(tmpMax)<fabs(dataQueue[j]))
        {
            tmpMax=dataQueue[j];
        }
    }
    return tmpMax;
}



unsigned char FilteringAlgorithm::recursiveAverage()
{
//#define N 12

//unsigned char value_buf[N];
//  unsigned char i;
//  unsigned char value;
//  int sum=0;

//  recursiveAverage[i++] = get_ad();       //采集到的数据放入最高位
//  for(i=0;i<N;i++)
//  {
//    recursiveAverage[i]=recursiveAverage[i+1];   //所有数据左移，低位扔掉
//    sum += recursiveAverage[i];
//  }
//  value = sum/N;
//  return(value);
}



double FilteringAlgorithm::medianMean(double dataIn,int bufferSize)
{//队列长度设置为偶数

//    qDebug()<<"FilteringAlgorithm::medianMean dataIn"<<dataIn;
    QMutexLocker locker(&filterMutex);
    dataQueue.enqueue(dataIn);
    while(dataQueue.size()>bufferSize)
    {
        dataQueue.dequeue();
    }

    int queueSize=dataQueue.size();
    if(queueSize<bufferSize|| 4>bufferSize)
    {
        return dataIn;
    }
    QQueue<double> tmpQueue=dataQueue;
    int i,j;
    double temp,sum=0,value;

//    qDebug()<<"1tmpQueue"<<tmpQueue;
    //采样值从小到大排列（冒泡法）
    for(j=0;j<queueSize-1;j++)
    {
        for(i=0;i<queueSize-1-j;i++)
        {
              if(tmpQueue[i]>tmpQueue[i+1])
              {
                temp = tmpQueue[i];
                tmpQueue[i] = tmpQueue[i+1];
                tmpQueue[i+1] = temp;
              }
        }
    }
//    qDebug()<<"2tmpQueue"<<tmpQueue;
    for(i=1;i<queueSize-1;i++)
    {
         sum += tmpQueue[i];
    }


    value = sum/(queueSize-2);
//    qDebug()<<"out "<<value;
    return(value);
}

double FilteringAlgorithm::medianMean_angle(double dataIn, int bufferSize)
{
    QMutexLocker locker(&filterMutex);
    dataQueue.enqueue(dataIn);
    while(dataQueue.size()>bufferSize)
    {
        dataQueue.dequeue();
    }

    int queueSize=dataQueue.size();
    if(queueSize<bufferSize || 4>bufferSize)
    {
        return dataIn;
    }
    //转换为角度距离
    QQueue<double> tmpQueue;
    for(int i=1;i<queueSize;i++)
    {
//        tmpQueue.enqueue(angleRotationJudgement(dataQueue.front(),dataQueue[i]));
        tmpQueue.enqueue(angleRotationJudgement(dataQueue[i],dataQueue.front()));
    }

    //滤波算法
    int i,j;
    double temp,sum=0,value;
    queueSize=tmpQueue.size();
//    qDebug()<<"1tmpQueue"<<tmpQueue;
    //采样值从小到大排列（冒泡法）
    for(j=0;j<queueSize-1;j++)
    {
        for(i=0;i<queueSize-1-j;i++)
        {
              if(tmpQueue[i]>tmpQueue[i+1])
              {
                temp = tmpQueue[i];
                tmpQueue[i] = tmpQueue[i+1];
                tmpQueue[i+1] = temp;
              }
        }
    }
//    qDebug()<<"2tmpQueue"<<tmpQueue;
    for(i=1;i<queueSize-1;i++)
    {
         sum += tmpQueue[i];
    }


    value = sum/(queueSize-2);
//    qDebug()<<"out "<<value;
    return transformToPi(dataQueue.front()+value);




}

double FilteringAlgorithm::transformToPi(double angleIn)
{
    //    int turns=angleIn/M_PI/2;//可能bug 参考getTurnsFromRad
        int turns;

        if(0<angleIn)
        {
            turns=(angleIn+M_PI) /(2*M_PI);
        }
        else
        {
            turns=(angleIn-M_PI) /(2*M_PI);
        }

        //    angleIn=angleIn-turns*M_PI;//错误
            angleIn=angleIn-turns*M_PI*2;

        if(angleIn>=-M_PI && angleIn<=M_PI)
        {
            return angleIn;
        }
        else if(angleIn<-M_PI)
        {
            return angleIn+2*M_PI;
        }
        else if(angleIn>M_PI)
        {
            return angleIn-2*M_PI;
        }

        return angleIn;
}


double FilteringAlgorithm::angleRotationJudgement(double targetAngle, double initialAngle)
{
    double angle1,angle2,angle3;
    angle1=targetAngle-initialAngle;
    angle2=angle1+2*M_PI;
    angle3=angle1-2*M_PI;
    double minNum;
    if(fabs(angle1)<=fabs(angle2))
    {
        minNum=angle1;
    }
    else
    {
        minNum=angle2;
    }

    if(fabs(angle3)<=fabs(minNum))
    {
        minNum=angle3;
    }

    return minNum;

}

